
#include "sync.h"


int cfg_rc_st_enable(node_mid_table* table,u8* pkt,u32 sync_mode)
{
	int i = 0;
	int ret = 0;
	u8 local_mac[6]={0};
	u32 *cfg_data = NULL;
	u32 st_enable_value = 0;

	sync_mode = sync_mode << 31;
	st_enable_value = sync_mode | ST_ENABLE ;

	printf("st_enable_value = %x \n",st_enable_value);
	write_debug_msg("st_enable_value = %x \n",st_enable_value);

	cfg_data = (u32 *)(pkt + sizeof(tsmp_header) + sizeof(tsmp_set_req_or_get_res_pkt_data));//配置的数据

		
	//依次配置每个HCP_CFG_REG寄存器
	for(i =0 ; i<table->num;i++)
	{
		printf("cfg cfg_st_enable ! mid = %d\n",table->node_mid[i]);
		write_debug_msg("cfg cfg_st_enable ! mid = %d\n",table->node_mid[i]);

		cfg_data[0] = htonl(st_enable_value);
		get_hcp_mac_from_mid(local_mac,table->node_mid[i]);
		tsmp_set_req(local_mac,1,htonl(HCP_CFG_REG),pkt);
		
		// 不能验证，主时钟寄存器为80 00 00 03，从时钟寄存器为80 00 00 07
		ret = rc_st_cfg_varify(local_mac,1,htonl(HCP_CFG_REG),pkt);
		if(ret == -1)
		{
			printf("varify cfg_rc_st_enable error! mid = %d\n",table->node_mid[i]);
			// return -1;
		}
	}

	return 0;
}

int cfg_rc_enable(node_mid_table* table,u8* pkt,u32 sync_mode)
{
	int i = 0;
	int ret = 0;
	u8 local_mac[6]={0};
	u32 *cfg_data = NULL;
	u32 rc_enable_value = 0;

	sync_mode = sync_mode << 31;
	rc_enable_value = sync_mode | RC_ENABLE ;

	printf("rc_enable_value = %x \n",rc_enable_value);
	write_debug_msg("rc_enable_value = %x \n",rc_enable_value);

	cfg_data = (u32 *)(pkt + sizeof(tsmp_header) + sizeof(tsmp_set_req_or_get_res_pkt_data));//配置的数据

		
	//依次配置每个HCP_CFG_REG寄存器
	for(i =0 ; i<table->num;i++)
	{
		printf("cfg cfg_rc_enable ! mid = %d\n",table->node_mid[i]);
		write_debug_msg("cfg cfg_rc_enable ! mid = %d\n",table->node_mid[i]);

		cfg_data[0] = htonl(rc_enable_value);
		get_hcp_mac_from_mid(local_mac,table->node_mid[i]);
		tsmp_set_req(local_mac,1,htonl(HCP_CFG_REG),pkt);
		ret = rc_cfg_varify(local_mac,1,htonl(HCP_CFG_REG),pkt);
		if(ret == -1)
		{
			printf("varify cfg_rc_enable error! mid = %d\n",table->node_mid[i]);
			return -1;
		}
		
	}

	return 0;
}

int cfg_cycle_len_enable(node_mid_table* table,u8* pkt,u32 enable_value)
{
	int i = 0;
	int ret = 0;
	u8 local_mac[6]={0};
	u32 *cfg_data = NULL;
	u32 rc_enable_value = 0;


	printf("cycle_len_enable_value = %x \n",enable_value);
	write_debug_msg("cycle_len_enable_value = %x \n",enable_value);

	cfg_data = (u32 *)(pkt + sizeof(tsmp_header) + sizeof(tsmp_set_req_or_get_res_pkt_data));//配置的数据

		
	//依次配置每个CC_CYCLELEN_ENABLE寄存器
	for(i =0 ; i<table->num;i++)
	{
		printf("cfg cycle_len_enable ! mid = %d\n",table->node_mid[i]);
		write_debug_msg("cfg cycle_len_enable ! mid = %d\n",table->node_mid[i]);

		cfg_data[0] = htonl(enable_value);
		get_hcp_mac_from_mid(local_mac,table->node_mid[i]);
		tsmp_set_req(local_mac,1,htonl(CC_CYCLELEN_ENABLE),pkt);
		ret = cfg_varify(local_mac,1,htonl(CC_CYCLELEN_ENABLE),pkt);
		if(ret == -1)
		{
			printf("varify cycle_len_enable error! mid = %d\n",table->node_mid[i]);
			return -1;
		}
		
	}

	return 0;
}


int cfg_sync_mode(node_mid_table* table,u8* pkt,u32 sync_mode)
{
	int i = 0;
	int ret = 0;
	u8 local_mac[6]={0};
	u32 *cfg_data = NULL;

	printf("sync_mode = %d \n",sync_mode);

	cfg_data = (u32 *)(pkt + sizeof(tsmp_header) + sizeof(tsmp_set_req_or_get_res_pkt_data));//配置的数据

		
	//依次配置每个交换机的同步模式
	for(i =0 ; i<table->num;i++)
	{
		printf("cfg cfg_sync_mode ! mid = %d\n",table->node_mid[i]);
		
		if(sync_mode == 1)
		{
			cfg_data[0] = htonl(0x80000000);
		}
		else
		{
			cfg_data[0] = htonl(0);
		}

		get_hcp_mac_from_mid(local_mac,table->node_mid[i]);
		tsmp_set_req(local_mac,1,htonl(HCP_CFG_REG),pkt);
		ret = cfg_varify(local_mac,1,htonl(HCP_CFG_REG),pkt);
		if(ret == -1)
		{
			printf("varify cfg_sync_mode error! mid = %d\n",table->node_mid[i]);
			return -1;
		}
		
	}

	return 0;
}


//配置和校验cc-basetime寄存器，成功则返回0，否则返回-1
int cfg_cc_base_time(node_mid_table* table,u8* pkt,int tsn_type)
{
	u8 local_mac[6]={0};
	u32 *read_data = NULL;
	u32 *cyclelen = NULL;
	u32 *cfg_data = NULL;
	u32 time_0_s = 0;
	u32 time_1_s = 0;
	u32 time_2_ns = 0;
	u32 cycle_length=0;
	int ret = 0;
	int i = 0;
	u64 time_ns = 0;
	u8 *debug_data = NULL;
	
	get_hcp_mac_from_mid(local_mac,table->node_mid[0]);
	
	//(1)读请求，获取当前全局时间
	tsmp_get_req(local_mac,3,htonl(SYNC_TIME_H),pkt);
	read_data = tsmp_get_res(local_mac,3,htonl(SYNC_TIME_H));//读响应

	if(read_data == NULL)
		{
			write_debug_msg("cfg_cc_base_time tsmp_get_res error");
			printf("cfg_cc_base_time tsmp_get_res error");
			return -1;
		}
	
	printf("read_data = %x ,%x,%x\n",ntohl(read_data[0]),ntohl(read_data[1]),ntohl(read_data[2]));//全局时间：高64位为s,低32位为ns

	time_0_s = ntohl(read_data[0]);//高32bit的全局时间，主机序
	time_1_s = ntohl(read_data[1]);//中32bit的全局时间，主机序
	time_2_ns= ntohl(read_data[2]);//低32bit的全局时间，主机序
	

	//(2)读请求，获取门控调度的循环周期长度cyclelen
	tsmp_get_req(local_mac,1,htonl(CC_CYCLELEN_REG),pkt);
	cyclelen = tsmp_get_res(local_mac,1,htonl(CC_CYCLELEN_REG));//读响应
	if(cyclelen == NULL)
		{
			write_debug_msg("cyclelen tsmp_get_res error");
			printf("cyclelen tsmp_get_res error");
			return -1;
		}
	
	cycle_length= ntohl(cyclelen[0]);//cycle_length长度，主机序
	if(cycle_length == 0)
		{
			write_debug_msg("cycle_length =0,error!\n");
			printf("cycle_length =0,error!\n");
			return -1;
		}

	write_debug_msg("1. time_0_s = %x, time_1_s = %x, time_2_ns = %x , cyclelen = %x    \n", time_0_s, time_1_s, time_2_ns, cycle_length);
	
	//（2）配置和校验cc-basetime寄存器

	cfg_data = (u32 *)(pkt + sizeof(tsmp_header) + sizeof(tsmp_set_req_or_get_res_pkt_data));//配置的数据
	
	//配置时间转换成cyclelen的整数倍，ns级别
	time_ns=time_0_s;
	time_ns=((time_ns<<32)+time_1_s)*1000000000+time_2_ns+CC_WAITTIME;
	time_ns=(time_ns/cycle_length)*cycle_length; //待配置的时间，长度为整数倍cyclelen,单位ns
	write_debug_msg("2.time_ns = %llx\n",time_ns);
	
	//仿真环境,0为FPGA，1为SIM
	if(!tsn_type)
	{
		//重新分配时间至各个内存单元
		time_0_s  = (time_ns/1000000000)>>32;
		time_1_s  = (time_ns/1000000000)&0xffffffff;
		time_2_ns = time_ns%1000000000;

		write_debug_msg("3. time_0_s = %x, time_1_s = %x, time_2_ns = %x    \n", time_0_s, time_1_s, time_2_ns);
	}
	else
	{
		//仿真环境下时间调整
		time_ns=time_ns*10000;
		//重新分配时间至各个内存单元
		time_0_s  = (time_ns/1000000000)>>32;
		time_1_s  = (time_ns/1000000000)&0xffffffff;
		time_2_ns = time_ns%1000000000;
		
	}
	//依次配置每个cc-basetime寄存器
	int j = 0;
	
	for(i =0 ; i<table->num;i++)
	{
		printf("cfg CC_BASETIME_H ! mid = %d\n",table->node_mid[i]);
		write_debug_msg("cfg CC_BASETIME_H ! mid = %d\n",table->node_mid[i]);

		cfg_data[0] = htonl(time_0_s); //高32bit的全局时间，网络序
		cfg_data[1] = htonl(time_1_s); //中32bit的全局时间，网络序
		cfg_data[2] = htonl(time_2_ns); //低32bit的全局时间，网络序

		write_debug_msg("4. cfg_data[0] = %x, cfg_data[1] = %x, cfg_data[2] = %x    \n", cfg_data[0], cfg_data[1], cfg_data[2]);

		debug_data = (pkt + sizeof(tsmp_header) + sizeof(tsmp_set_req_or_get_res_pkt_data));
		write_debug_msg("5. ");

		for (j = 0; j < 12; j++)
		{
			write_debug_msg("%x ", debug_data[j]);
		}
		write_debug_msg("\n");
		
		get_hcp_mac_from_mid(local_mac,table->node_mid[i]);
		tsmp_set_req(local_mac,3,htonl(CC_BASETIME_H),pkt);		
		ret = cfg_varify(local_mac,3,htonl(CC_BASETIME_H),pkt);
		if(ret == -1)
		{
			printf("varify CC_BASETIME_H error! mid = %d\n",table->node_mid[i]);
			return -1;
		}
		
	}
	//配置和校验CC_BASETIME寄存器

	//sleep(5);
	
	return 0;
	
}


//若syn_ok使能为1，则返回1，否则返回0
int ether_syn_ok(node_mid_table* table,u8* pkt,u32 sync_mode)
{
	u8 local_mac[6]={0};
	u32 *read_data = NULL;
	u32 sync_ok_value = 0;
	int i;
	int syn_flag = 0;

	sync_mode = sync_mode << 31;
	sync_ok_value = 0x80000005;
	printf("sync_ok_value = %x \n",sync_ok_value);

	for(i =0 ; i<table->num;i++)
	{
		while(syn_flag == 0)
		{
			get_hcp_mac_from_mid(local_mac,table->node_mid[i]);

			//读请求
			tsmp_get_req(local_mac,1,htonl(HCP_CFG_REG),pkt);
			read_data = tsmp_get_res(local_mac,1,htonl(HCP_CFG_REG));//读响应

			if(read_data == NULL)
			{
				write_debug_msg("ether_syn_ok tsmp_get_res error");
				printf("ether_syn_ok tsmp_get_res error");
				continue;
			}

			printf("read_data = %x \n",ntohl(read_data[0]));

			if( ((ntohl(read_data[0])) & 0x00000004) == 4)
			{
				syn_flag = 1;
				continue;
			}

			printf("read_data = %x \n",ntohl(read_data[0]));
			printf("sync_ok_value = %x \n",sync_ok_value);
			printf("syn_flag = %x \n",syn_flag);

			sleep(1);
		}
		
		syn_flag = 0;
	}

	return 1;
}




int startup_sync_exe(u8 *network_inetrface)
{
	char tmp_buf[64] = {0};

	sprintf(tmp_buf, "sh %s %s", PTP_EXE_SHELL_FILENAME, network_inetrface);
	system(tmp_buf);

	printf("*************SYNC_EXE OK********************\n");
}

int sync_start(u8 *network_inetrface,node_mid_table* table,int tsn_type,u32 sync_mode)
{
	//申请空间，用于构造配置报文
	u8 *cfg_pkt = NULL;
	int ret = 0;
	int i = 0;
	u32 cc_enable = 0;

	printf("*************start SYNC_EXE********************\n");
	write_debug_msg("*************start SYNC_EXE********************\n");

	cfg_pkt = (u8 *)malloc(MAX_PKT_LEN);
	if(NULL == cfg_pkt)
	{
		printf("sync_start,malloc buf fail\n");
		return -1;
	}
	
	bzero(cfg_pkt,MAX_PKT_LEN);

	// (1)配置时间同步模式以及RC使能
	ret = cfg_rc_enable(table, cfg_pkt, sync_mode); 
	if(ret == -1)
	{
		printf("cfg_rc_enable error!\n");

		return -1;
	}

	//(2) 配置CC使能为0
	ret = cfg_cycle_len_enable(table, cfg_pkt,cc_enable);
	if(ret == -1)
	{
		printf("cfg_cycle_len_enable error!\n");

		return -1;
	}
		
	// (3)开启时间同步程序	
    if(tsn_type)
    {
		sleep(5);
    	write_debug_msg("TSN_SIM!!!! \n");
		startup_sync_exe("2");
    }
    else
	{
		write_debug_msg("TSN_FPGA!!!! \n");
		startup_sync_exe(network_inetrface);
	}

	//(4)等待全网时间同步

	while(ret == 0)
	{
		sleep(2);
		ret = ether_syn_ok(table,cfg_pkt,sync_mode);		
	}
	
	bzero(cfg_pkt,MAX_PKT_LEN);	
	
	// (5)配置CC使能为1
	cc_enable = 1;
	ret = cfg_cycle_len_enable(table, cfg_pkt,cc_enable);
	if(ret == -1)
	{
		printf("cfg_cycle_len_enable error!\n");
		write_debug_msg("cfg_cycle_len_enable error!\n");
		return -1;
	}


	ret = cfg_rc_st_enable(table, cfg_pkt, sync_mode);
	if(ret == -1)
	{
		printf("cfg_rc_st_enable error!\n");

		return -1;
	}
	
	//(6)配置cc_basetime寄存器
	ret = cfg_cc_base_time(table,cfg_pkt,tsn_type);
	if(ret == -1)
	{
		printf("cfg_cc_base_time error!\n");

		return -1;
	}
	else
	{
		write_debug_msg("cfg_cc_base_time ok!\n");
		sleep(2);
	}

	free(cfg_pkt);

	return 0;

}




